﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using System.Diagnostics;
using PloobsEngine.Utils;
using PloobsEngine.Behaviors;
using PloobsEngine.SceneControl;

namespace PloobsEngine.IA
{
    public class FollowPathActuator : IActuator
    {
        public FollowPathActuator(IObject character)
        {
            this.obj = character;
            this.Name = "FollowPathActuator";
            resp.ActuatorName = Name;
        }

        private IObject obj;
        private Vector3 start;
        private Vector3 end;
        private float distanceError = 0.3f;
        ActuatorSensorInformation resp = new ActuatorSensorInformation();
        bool b = false;

        private float minAngleErrorToTurn = 1;

        /// <summary>
        /// em graus
        /// </summary>
        public float MinAngleErrorToTurn
        {
            get { return minAngleErrorToTurn; }
            set { minAngleErrorToTurn = value; }
        }

        public float DistanceError
        {
            get { return distanceError; }
            set { distanceError = value; }
        }

        public void SetPath(Vector3 start, Vector3 end)
        {
            resp.Ended = false;
            this.start = start;
            this.end = end;
        }

        public override ActuatorSensorInformation PerformAct()
        {

            Vector2 direction = VectorUtils.ToVector2(end) - VectorUtils.ToVector2(obj.Position);
            Vector3 direction3d = (end - obj.Position);

            Vector2 objpos = new Vector2(obj.Position.X, obj.Position.Z);
            Vector2 endpos = new Vector2(end.X, end.Z);

            float enddistance = direction.Length();
            if (enddistance < distanceError)
            {
                resp.Ended = true;
                return resp;
            }

            Vector2 objhead = VectorUtils.ToVector2(obj.PhysicObject.FaceVector);
            direction.Normalize();
            objhead.Normalize();
            direction3d.Normalize();

            float angle = (float)Math.Acos(Vector2.Dot(objhead, direction));


            //////TO REMOVE - DEBUG !!!!
            //DebugVectorDraw.ClearVectorList();
            //DebugVectorDraw.addVector(start - Vector3.Up * 10, Vector3.Up * 50, Color.Yellow);
            //DebugVectorDraw.addVector(end - Vector3.Up * 10, Vector3.Up * 50, Color.YellowGreen);
            //DebugVectorDraw.addVector(obj.Position, 10 * obj.PhysicObject.FaceVector, Color.Black);
            //DebugVectorDraw.addVector(obj.Position, 10 * direction3d, Color.BlanchedAlmond);

            double x = VectorUtils.FindAngleBetweenTwoVectors2D(objhead, direction);
            if (Math.Abs(angle) > MathHelper.ToRadians(minAngleErrorToTurn) && b == true)
            {
                if(x < 0)
                {
                    ///Debug.WriteLine(" > " + angle);
                    obj.Behavior.StartBehavior(CharacterMovimentationBehavior.MOVERIGHT, CharacterMovimentationBehavior.MOVERIGHT,null);
                }
                else
                {
                    //Debug.WriteLine(" <= " +  angle);
                    obj.Behavior.StartBehavior(CharacterMovimentationBehavior.MOVELEFT, CharacterMovimentationBehavior.MOVELEFT, null);
                }
                //b = false;     -> Adicionar novamente
            }
            else
            {
                //Debug.WriteLine(" >< 8 " + angle);
                obj.Behavior.StartBehavior(CharacterMovimentationBehavior.MOVEFRONT, CharacterMovimentationBehavior.MOVEFRONT, null);
                b = true;
            }
            resp.Ended = false;
            return resp;
        }
    }
}
